#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/opengl_interop.hpp"
#include "opencv2/contrib/contrib.hpp"
#include <windows.h>
#include <GL/freeglut.h>
#include "customTypes.h"
#include "outlier.h"
#include "beehiveConfiguration.h"
#include "beehive.h"
#include <iostream>
#include <ctime>   // clock()
#include <map>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
using namespace cv;
using namespace std;

static void outputMatrix(const Mat mat)
{
	for(int i = 0; i < mat.rows; i++)
	{
		for(int j = 0; j < mat.cols; j++)
		{
			cout << mat.at<double>(i,j)<< " ";
		}
		cout<<endl;
	}
		cout<<endl;
		cout<<endl;
}


static bool readCameraMatrix(const string& filename,
                             Mat_<double>& cameraMatrix, Mat_<double>& distCoeffs,
                             Size& calibratedImageSize )
{
    FileStorage fs(filename, FileStorage::READ);
    fs["imageWidth"] >> calibratedImageSize.width;
    fs["imageHeight"] >> calibratedImageSize.height;
    fs["D"] >> distCoeffs;
    fs["K"] >> cameraMatrix;

	return true;
}


string getImagePath(string modelname, string nodename, string extension)
{
	string path = modelname;
	path.append("\\");
	path.append(nodename);
	path.append(".");
	path.append(extension);

	return path;
}

typedef Vec<double, 9> Vec9d;


static bool readModelViews( const string& filename, string modelname, vector<Point3f>& box,
                           vector<string>& imagelist, vector<Vec9d>& poseListRot, vector<Vec3d>& poseListTr)
{
    imagelist.resize(0);
    poseListRot.resize(0);
    poseListTr.resize(0);
    box.resize(0);

    FileStorage fs(filename, FileStorage::READ);
    if( !fs.isOpened() )
        return false;
    fs["box"] >> box;

    FileNode all = fs["images"];
    FileNodeIterator it = all.begin(), it_end = all.end();

	string extension = fs["extension"];

	for(; it != it_end; ++it)
    {
        FileNode n = *it;
		string name = getImagePath(modelname, n.name(), extension);
        imagelist.push_back(name);

		Mat_<double> rotationMat, translationMat, rotationRodrigues(3,1,CV_64F);
		n["R"]>>rotationMat;
		n["T"]>>translationMat;
		Vec9d rotVec = Vec9d(rotationMat.at<double>(0,0), rotationMat.at<double>(0,1), rotationMat.at<double>(0,2),
			rotationMat.at<double>(1,0), rotationMat.at<double>(1,1), rotationMat.at<double>(1,2),
			rotationMat.at<double>(2,0), rotationMat.at<double>(2,1), rotationMat.at<double>(2,2));

		Vec3d trVec = Vec3d(translationMat.at<double>(0,0), translationMat.at<double>(1,0), translationMat.at<double>(2,0));
		
		poseListRot.push_back(rotVec);
		poseListTr.push_back(trVec);
    }
	return true;
}

struct PointModel
{
    vector<Point3d> points;
    vector<vector<int> > didx;
    Mat descriptors;
    string name;
};

static void writeModel(const string& modelFileName, const string& modelname,
                       const PointModel& model)
{
    FileStorage fs(modelFileName, FileStorage::WRITE);

    fs << modelname << "{" <<
        "points" << "[:" << model.points << "]" <<
        "idx" << "[:";

    for( int i = 0; i < model.didx.size(); i++ )
        fs << "[:" << model.didx[i] << "]";
    fs << "]" << "descriptors" << model.descriptors;
}


static void unpackPose(const Vec9d& poseRot,const Vec3d& poseTr, Mat& R, Mat& t)
{
    R = (Mat_<double>(3,3) << poseRot[0], poseRot[1], poseRot[2],poseRot[3], poseRot[4], poseRot[5],poseRot[6], poseRot[7], poseRot[8]);
    t = (Mat_<double>(3,1) << poseTr[0], poseTr[1], poseTr[2]);
}

static void findConstrainedCorrespondences(const vector<KeyPoint>& keypoints1,
                const vector<KeyPoint>& keypoints2,
                const Mat& descriptors1,
                const Mat& descriptors2,
                vector<Pair2i>& matches,
				double ratio)
{
    int dsize = descriptors1.cols;

    matches.clear();

    for( int i = 0; i < (int)keypoints1.size(); i++ )
    {
        Point2f p1 = keypoints1[i].pt;
        double bestDist1 = DBL_MAX, bestDist2 = DBL_MAX;
        int bestIdx1 = -1;
        const float* d1 = descriptors1.ptr<float>(i);

        for( int j = 0; j < (int)keypoints2.size(); j++ )
        {
            Point2f p2 = keypoints2[j].pt;
            const float* d2 = descriptors2.ptr<float>(j);
            double dist = 0;

            for( int k = 0; k <= dsize; k ++ )
            {
                float t0 = d1[k] - d2[k];
                dist += t0*t0;

                if( dist >= bestDist2 )
                    break;
            }

            if( dist < bestDist2 )
            {
                if( dist < bestDist1 )
                {
                    bestDist2 = bestDist1;
                    bestDist1 = dist;
                    bestIdx1 = (int)j;
                }
                else if( dist < bestDist2 )
                {
                    bestDist2 = dist;
                }
            }
        }

        if( bestIdx1 >= 0 && bestDist1 < bestDist2*ratio )
        {
            Point2f p2 = keypoints2[bestIdx1].pt;
            
			double threshold = bestDist1/ratio;
            
			const float* d22 = descriptors2.ptr<float>(bestIdx1);
            int i1 = 0;
            for( ; i1 < (int)keypoints1.size(); i1++ )
            {
                if( i1 == i )
                    continue;
                Point2f pt1 = keypoints1[i1].pt;
                const float* d11 = descriptors1.ptr<float>(i1);
                double dist = 0;

                for( int k = 0; k < dsize; k++ )
                {
                    float t = d11[k] - d22[k];
                    dist += t*t;
                    if( dist >= threshold )
                        break;
                }

                if( dist < threshold )
                    break;
            }
            if( i1 == (int)keypoints1.size() )
                matches.push_back(Pair2i(i,bestIdx1));
        }
    }
}

static Mat_<double> calculateProjectionMatrix(Mat_<double> R, Mat_<double> t, Mat_<double> K)
{
	Mat_<double> Rt(3,4);

	for(int i = 0; i < 4; i++)
	{
		for(int j = 0; j < 3; j++)
		{
			if(i == 3)
			{
				double tj = t.at<double>(j,0);
				Rt.at<double>(j,3) = tj;
			}
			else
			{
				double rij = R.at<double>(i,j);
				Rt.at<double>(i,j) = rij;
			}
		}
	}

	return K*Rt;
}

static Point3d triangulatePointsPair(const vector<Point2f>& pts,
                                const vector<Mat>& PMats)
{
	if(pts.size() != 2 || PMats.size() != 2)
	{
		throw;
	}

	Mat_<double> A(4,4);
	for(int i = 0; i < 2; i++)
	{
		double u = pts[i].x;
		double v = pts[i].y;
		Mat_<double> P = PMats.at(i);
		for(int j = 0; j < 2; j++)
		{
			double factor = u;
				if(j == 1)
					factor = v;
			for(int k = 0; k < 4; k++)
			{
				A.at<double>(i*2+j,k) = (factor*P.at<double>(2,k)) - P.at<double>(j,k);
			}
		}
	}
	double xData[4];

	Mat_<double> X(4,1,xData);

	SVD::solveZ(A,X);

	double x = X.at<double>(0,0);
	double y = X.at<double>(1,0);
	double z = X.at<double>(2,0);
	double w = X.at<double>(3,0);

	return Point3d(x/w, y/w, z/w);
}

static bool isTriangulationGood(Point3d& triangulatedPoint, 
								const vector<Point2f> pairPts,
								const vector<Mat>& RMats, 
								const vector<Mat>& tMats, 
								const Mat& cameraMatrix)
{
	vector<Point3f> objpts;
	objpts.push_back(triangulatedPoint);
	vector<Point2f> imgpts1, imgpts2;
	projectPoints(Mat(objpts), RMats[0], tMats[0], cameraMatrix, Mat(), imgpts1);
	projectPoints(Mat(objpts), RMats[1], tMats[1], cameraMatrix, Mat(), imgpts2);

	double e1 = norm(imgpts1[0] - pairPts[0]);
	double e2 = norm(imgpts2[0] - pairPts[1]);
    if( e1 + e2 < 5)
	{
		return true;
	}

	return false;
}

static Point3d triangulatePoints(const vector<Point2f>& pts,
                                const vector<Mat>& PMats, vector<Mat>& RMats, vector<Mat>& tMats, Mat& cameraMatrix)
{
	int numberOfPoints = pts.size();
	if(numberOfPoints != PMats.size())
	{
		throw;
	}

	vector<Mat> pairMatsP(2);
	vector<Mat> pairMatsR(2);
	vector<Mat> pairMatsT(2);
	vector<Point2f> pairPts(2);
	vector<Point3d> triangulatedPoints;

	for(int i = 0; i < numberOfPoints - 1; i++)
	{
		for(int j = i + 1; j < numberOfPoints; j++)
		{
			pairMatsR.clear();
			pairMatsT.clear();
			pairMatsP.clear();
			pairPts.clear();

			pairMatsP.push_back(PMats[i]);
			pairMatsP.push_back(PMats[j]);

			pairMatsR.push_back(RMats[i]);
			pairMatsR.push_back(RMats[j]);

			pairMatsT.push_back(tMats[i]);
			pairMatsT.push_back(tMats[j]);

			pairPts.push_back(pts[i]);
			pairPts.push_back(pts[j]);

			Point3d triangulatedPoint = triangulatePointsPair(pairPts, pairMatsP);
			
			if(isTriangulationGood(triangulatedPoint, pairPts, pairMatsR, pairMatsT, cameraMatrix))
			{
				triangulatedPoints.push_back(triangulatedPoint);
			}

			
		}
	}

	int numberOfPairs = triangulatedPoints.size();
	Point3d finalTriangulatedPoint;
	for(int i = 0; i < numberOfPairs; i++)
	{
		finalTriangulatedPoint = finalTriangulatedPoint + triangulatedPoints[i];
	}

	double factor = 1.0 / numberOfPairs;

	return finalTriangulatedPoint * factor;
}

static void visualizePointCloud(int nimages,
						   const vector<string>& imageList,
						   const vector<Mat>& Rs,
                           const vector<Mat>& ts,
                           const Mat& cameraMatrix,
						   PointModel& model)
{
    Mat img(640,480,CV_8UC3);
    vector<Point2d> imagePoints;
    namedWindow("Test", 1);

    // visualize the cloud
    for( int i = 0; i < nimages; i++ )
    {
        //img = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)i), 1);
        img = imread(imageList[i], 1);
        projectPoints(Mat(model.points), Rs[i], ts[i], cameraMatrix, Mat(), imagePoints);

        for( int k = 0; k < (int)imagePoints.size(); k++ )
		{
            circle(img, imagePoints[k], 2, Scalar(0,255,0), -1, CV_AA, 0);
		}

        imshow("Test", img);
        int c = waitKey();
        if( c == 'q' || c == 'Q' )
            break;
    }
}

static void showMatchedKeypointsInfo(string firstImage, string secondImage, double deltaPercent,
							  const vector<KeyPoint>& keypoints1,
							  const vector<KeyPoint>& keypoints2,
							  vector<Pair2i>& pairwiseMatches, bool visual)
{
	if(visual)
	{
		namedWindow("img1");
		namedWindow("img2");
	}
	
	Mat img1, img2;
	if(visual)
	{
		img1 = imread(firstImage, CV_LOAD_IMAGE_COLOR);
		img2 = imread(secondImage, CV_LOAD_IMAGE_COLOR);
	}
			
	int pairsFoundBetweenTwoPictures = 0;

	for( int k = 0; k < pairwiseMatches.size(); k++ )
    {
		int i1 = pairwiseMatches[k].first, i2 = pairwiseMatches[k].second;
		pairsFoundBetweenTwoPictures++;

		if(visual)
		{
			const Point2f* kpt1 = &keypoints1[i1].pt;
			const Point2f* kpt2 = &keypoints2[i2].pt;

			
			Scalar color(rand()%256,rand()%256, rand()%256);
			circle(img1, *kpt1 , 2, color, -1, CV_AA);
			circle(img2, *kpt2 , 2, color, -1, CV_AA);
		}
    }

	if(visual)
	{
		imshow("img1", img1);
		imshow("img2", img2);
		waitKey();
	}

	cout<<"Pic 1: '"<<firstImage<<"' Pic 2: '"<<secondImage<<"' Delta T: "<<deltaPercent<<" Matches: "<<pairsFoundBetweenTwoPictures<<endl<<endl;
}

static void removeOutliers(const vector<KeyPoint>& keypoints1,
					const vector<KeyPoint>& keypoints2,
					const vector<Mat>& Ps,
					const vector<Mat>& Rs,
                    const vector<Mat>& ts,
					const Mat_<double> cameraMatrix,
					vector<Pair2i>& pairwiseMatches,
					int& outliersFound)
{

	
	BeeHive hive(&pairwiseMatches, &keypoints1, &keypoints2);
	hive.init(1, 10, 30.0, 60.0, 70.0);
	hive.exploreBigValey();

	hive.init(1, 10, 30.0, 40.0, 80.0);
	hive.exploreBigValey();

	outliersFound+= hive.outliers.size();


    vector<Point2f> pts_k(2);

	for( int k = pairwiseMatches.size() - 1; k > -1; k-- )
    {
		int i1 = pairwiseMatches[k].first, i2 = pairwiseMatches[k].second;

        pts_k[0] = keypoints1[i1].pt;
        pts_k[1] = keypoints2[i2].pt;
       
        Point3d objpt = triangulatePointsPair(pts_k, Ps);

		if(isTriangulationGood(objpt, pts_k, Rs, ts, cameraMatrix) == false)
		{
			pairwiseMatches.erase(pairwiseMatches.begin()+k);
			outliersFound++;
		}
	}
}

void mergeClusters(vector<vector<int>> clustersInt, int firstIndex, int secondIndex)
{
	if(firstIndex > secondIndex)
	{
		int temp = firstIndex;
		firstIndex = secondIndex;
		secondIndex = temp;
	}

	vector<int> first = clustersInt.at(firstIndex);
	vector<int> second = clustersInt.at(secondIndex);

	for(int i = 0; i < second.size(); i++)
	{
		if(std::find(first.begin(), first.end(), second[i]) == first.end()) 
		{
			first.push_back(second[i]);
		}
	}

	clustersInt.erase(clustersInt.begin() + secondIndex);
}

static vector<vector<Pair2i>> clusterizePairwiseMatches(vector<Pair2i> pairs, SetIntPair keypointsIdxMap)
{
    vector<vector<int>> clustersInt;
	for( int i = 0; i < pairs.size(); i++)
	{
		Pair2i current = pairs[i];
		int foundFirst = -1;
		int foundSecond = -1;
		for(int j = 0; j < clustersInt.size(); j++)
		{
			for(int k = 0; k < clustersInt[j].size(); k++)
			{
				if(clustersInt[j][k] == current.first)
				{
					foundFirst = j;
				}
				else if (clustersInt[j][k] == current.second)
				{
					foundSecond = j;
				}
			}
			if(foundFirst > -1 && foundSecond > -1)
			{
				break;
			}
			
		}
		
		if(foundFirst > -1 && foundSecond > -1)
		{
			if(foundFirst != foundSecond)
			{
				mergeClusters(clustersInt, foundFirst, foundSecond);
			}
		}
		else if(foundFirst > -1)
		{
			clustersInt[foundFirst].push_back(current.second);
		}
		else if (foundSecond > -1)
		{
			clustersInt[foundSecond].push_back(current.first);
		}
		else
		{
			vector<int> newVect;
			newVect.push_back(current.first);
			newVect.push_back(current.second);
			clustersInt.push_back(newVect);
		}
	}

	vector<vector<Pair2i>> clusters(clustersInt.size());

	for( int i = 0; i < clustersInt.size(); i++ )
	{
		vector<int> current = clustersInt[i];
		for(int j = 0; j < current.size(); j++)
		{
			clusters[i].push_back(keypointsIdxMap[current[j]]);
		}
	}

	return clusters;
}




static void calculateKeyPointsAndDescriptors(const Ptr<FeatureDetector>& detector,
											 const Ptr<DescriptorExtractor>& descriptorExtractor,
											 const vector<string>& imageList, 
											 vector<vector<KeyPoint>>& allkeypoints,
											 vector<int>& dstart,
											 int& descriptorSize,
											 vector<float>& alldescriptorsVec )
{
    int nimages = imageList.size();
    Mat descriptorbuf;

    for( int i = 0; i < nimages; i++ )
    {
        Mat img = imread(imageList[i], 1), gray;
        cvtColor(img, gray, CV_BGR2GRAY);
        
		vector<KeyPoint> keypoints;
		detector->detect(gray, keypoints);

        descriptorExtractor->compute(gray, keypoints, descriptorbuf);
        allkeypoints.push_back(keypoints);

        Mat buf = descriptorbuf;
        if( !buf.isContinuous() || buf.type() != CV_32F )
        {
            buf.release();
            descriptorbuf.convertTo(buf, CV_32F);
        }
        descriptorSize = buf.cols;

        int prev = alldescriptorsVec.size();
        int delta = buf.rows*buf.cols;
        alldescriptorsVec.resize(prev + delta);
        std::copy(buf.ptr<float>(), buf.ptr<float>() + delta,
                  alldescriptorsVec.begin() + prev);
        dstart.push_back(dstart.back() + (int)keypoints.size());

    }
}

static void findPairwiseCorrespondencies(const vector<string>& imageList, 
										 const vector<Point3f>& modelBox,
										 const vector<int>& dstart,
										 const vector<Mat>& Rs,
 										 const vector<Mat>& ts,
	 									 const vector<Mat>& Ps,
										 const vector<vector<KeyPoint>>& allkeypoints,
										 const Mat& alldescriptors,
										 const Mat_<double>& cameraMatrix,
								   	 	 SetIntPair& keypointsIdxMap,
										 vector<Pair2i>& pairs)

{

	int nimages = imageList.size();
    int pairsFound = 0;
    int outliersFound = 0;
    vector<Point2f> pts_k(2);
    vector<Mat> Rs_k(2), ts_k(2), Ps_k(2);
	vector<Pair2i> pairwiseMatches;
	
	double boxDiagonal = norm(modelBox.at(0)-modelBox.at(1)); //biggest possible translation in the model box.
    const double DescriptorRatio = 0.7;

    for( int i = 0; i < nimages; i++ )
        for( int j = i+1; j < nimages; j++ )
        {
			double deltaT = norm(ts[i] - ts[j]);
			double deltaPercent = (deltaT/boxDiagonal)*100;
			if(deltaPercent > 15.0)
				continue;

            const vector<KeyPoint>& keypoints1 = allkeypoints[i];
            const vector<KeyPoint>& keypoints2 = allkeypoints[j];
            Mat descriptors1 = alldescriptors.rowRange(dstart[i], dstart[i+1]);
            Mat descriptors2 = alldescriptors.rowRange(dstart[j], dstart[j+1]);

            findConstrainedCorrespondences( keypoints1, keypoints2,
                                            descriptors1, descriptors2,
                                            pairwiseMatches, DescriptorRatio);

			Rs_k[0] = Rs[i]; Rs_k[1] = Rs[j];
            ts_k[0] = ts[i]; ts_k[1] = ts[j];
            Ps_k[0] = Ps[i]; Ps_k[1] = Ps[j];

			removeOutliers(keypoints1, keypoints2, Ps_k, Rs_k, ts_k, cameraMatrix, pairwiseMatches, outliersFound);
			
			for( int k = 0; k < pairwiseMatches.size(); k++ )
            {
				int i1 = pairwiseMatches[k].first, i2 = pairwiseMatches[k].second;
                pairsFound++;

				pairs.push_back(Pair2i(i1+dstart[i], i2+dstart[j]));
				keypointsIdxMap[i1+dstart[i]] = Pair2i((int)i,i1);
                keypointsIdxMap[i2+dstart[j]] = Pair2i((int)j,i2);
            }
			
			//showMatchedKeypointsInfo(imageList[i], imageList[j], deltaPercent, keypoints1, keypoints2, pairwiseMatches, true);
        }

    printf("Ok.");
    printf("\nTotal pairs = %d", pairsFound );
    printf("\nTotal outliers = %d\n", outliersFound );
}

static void compute3dPoints(const vector<vector<KeyPoint>>& allkeypoints,
						   const Mat& alldescriptors,
						   const vector<int>& dstart,	 									 
						   const vector<Mat>& Ps,
						   const vector<Mat>& Rs,
						   const vector<Mat>& ts,
						   Mat cameraMatrix,
						   const vector<vector<Pair2i>>& clusters,
						   PointModel& model
						   )
{
	vector<Point2f> pts_k(2);
    vector<Mat> Ps_k(2);
    vector<Mat> Rs_k(2);
    vector<Mat> ts_k(2);
	
	int nclasses = clusters.size();

	int globalDIdx = 0;
    for( int k = 0; k < nclasses; k++ )
    {
        int i, n = (int)clusters[k].size();
        pts_k.resize(n);
        Ps_k.resize(n);
        Rs_k.resize(n);
        ts_k.resize(n);
        model.didx[k].resize(n);

		for( i = 0; i < n; i++ )
        {
            int imgidx = clusters[k][i].first, ptidx = clusters[k][i].second;

            Mat dstrow = model.descriptors.row(globalDIdx);
            alldescriptors.row(dstart[imgidx] + ptidx).copyTo(dstrow);

            model.didx[k][i] = globalDIdx++;
            pts_k[i] = allkeypoints[imgidx][ptidx].pt;
            Ps_k[i] = Ps[imgidx];
            Rs_k[i] = Rs[imgidx];
            ts_k[i] = ts[imgidx];
        }

        Point3d objpt = triangulatePoints(pts_k, Ps_k, Rs_k, ts_k, cameraMatrix);
		model.points[k] = objpt;
    }
}

static void build3dmodel( const Ptr<FeatureDetector>& detector,
                          const Ptr<DescriptorExtractor>& descriptorExtractor,
                          const vector<Point3f>& modelBox,
                          const vector<string>& imageList,
                          const vector<Vec9d>& poseListRot,
						  const vector<Vec3d>& poseListTr,
                          const Mat_<double>& cameraMatrix,
                          PointModel& model )
{

	model.points.clear();
    model.didx.clear();


    int nimages = imageList.size();
    int nimagePairs = (nimages - 1)*nimages/2;

    vector<Mat> Rs, ts, Ps;
    
    for( int i = 0; i < nimages; i++ )
	{
        Mat_<double> R, t, P;
        
		unpackPose(poseListRot[i],poseListTr[i], R, t);
		P = calculateProjectionMatrix(R, t, cameraMatrix);

        Rs.push_back(R);
        ts.push_back(t);
		Ps.push_back(P);
	}

	vector<vector<KeyPoint>> allkeypoints;
    int descriptorSize = 0;
    vector<float> alldescriptorsVec;
    vector<int> dstart;
	dstart.push_back(0);

	
	printf("\nComputing descriptors...");
	int startTicks = clock();

	//1. calculate keypoints and keypoint descriptors
	calculateKeyPointsAndDescriptors(detector, descriptorExtractor, imageList, allkeypoints, dstart, descriptorSize, alldescriptorsVec);

	Mat alldescriptors((int)alldescriptorsVec.size()/descriptorSize, descriptorSize, CV_32F,
                       &alldescriptorsVec[0]);

    printf("Ok.");
    printf("\nTotal images = %d.",(int)nimages);
    printf("\nTotal keypoints = %d\n", alldescriptors.rows);

	cout << "Time taken to comute descriptors: " << clock() - startTicks << endl;
    
	printf("\nFinding correspondences...");

	SetIntPair keypointsIdxMap;
    vector<Pair2i> pairs;

	startTicks = clock();
	
	// 2. find pairwise correspondences
	findPairwiseCorrespondencies(imageList, modelBox, dstart, Rs, ts, Ps, allkeypoints, alldescriptors,cameraMatrix,
		keypointsIdxMap, pairs);

	cout<<"Time taken to compute pairwise correspondences: " << clock()- startTicks << endl;
    
	// 3. build the keypoint clusters
	printf("\nClustering correspondences...");
	startTicks = clock();

	vector<vector<Pair2i>> clusters  = clusterizePairwiseMatches(pairs, keypointsIdxMap);
	int nclasses = clusters.size();

	printf("Ok.");
	printf("\nTotal classes (i.e. 3d points) = %d\n", nclasses );
	cout<<"Time taken to clusterize pairwise correspondencies: " << clock()-startTicks << endl;

    // 4. now compute 3D points corresponding to each cluster and fill in the model data
    printf("\nComputing 3D coordinates...");
	startTicks = clock();

    model.descriptors.create((int)keypointsIdxMap.size(), descriptorSize, CV_32F);
    model.didx.resize(nclasses);
    model.points.resize(nclasses);

	compute3dPoints(allkeypoints,alldescriptors,dstart,Ps,Rs,ts,cameraMatrix, clusters,model);
	printf("Ok.\n");

	cout << "Time taken to compute 3d coordinates from clusters: "<<clock() - startTicks << endl;

	//visualizePointCloud(nimages, imageList, Rs, ts, cameraMatrix, model);
}

void init(void)
{
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_FLAT);
}

void drawSphere(double x, double y, double z)
{
	glPushMatrix();
	glTranslated(x,y,z);
	glutSolidSphere(0.001,5,5);
	glPopMatrix();
}

PointModel model;

void drawSpheres()
{
	for(int i = 0; i < model.points.size();i++)
	{
		Point3d point = model.points.at(i);
		drawSphere(point.x,	point.y, point.z);
	}
}

double translateX = 0.0;
double translateY = 0.0;
double translateZ = 0.0;
double rotateAngle = 0.0;

double rotateX = 0.0;
double rotateY = 0.0;
double rotateZ = 0.0;


void display(void)
{
	glClear(GL_COLOR_BUFFER_BIT);
	glColor3f(255.0, 255.0, 255.0);
	glLoadIdentity(); /* clear the matrix */
	/* viewing transformation */
	gluLookAt(-0.0292149526928, 0.2141923869131, 0.52269561933, 0.0177525, -0.0118135, -0.0546675, 0.0, 1.0, 0.0);
	glScalef(1.0, 1.0, 1.0); /* modeling transformation */
	//glTranslated(0.0277525, 0.0418135, -0.0546675);
	glRotated(rotateAngle, rotateX, rotateY, rotateZ);
	drawSpheres();
	//glTranslated(0.0277525, 0.0418135, -0.0546675);
	glFlush();
}

void reshape(int w, int h)
{
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(30.0,1.0,0.05,1.0);
	//glFrustum(-1.0, 1.0, -1.0, 1.0, 1.5, 20.0);
	glMatrixMode(GL_MODELVIEW);
}

void keyPressed(unsigned char key, int x, int y)
{
	if (key == 'w')
	{
		rotateY = 0.0;
		rotateX = 1.0;
		rotateAngle+=10;
	}
    else if (key == 's')
	{
		rotateY = 0.0;
		rotateX = 1.0;
		rotateAngle-=10;
	}
    else if (key == 'a')
	{
		rotateY = 1.0;
		rotateX = 0.0;
		rotateAngle-=10;
	}
    else if (key == 'd')
	{
		rotateY = 1.0;
		rotateX = 0.0;
		rotateAngle+=10;
	}

    glutPostRedisplay();
}


int main(int argc, char** argv)
{
	initModule_nonfree();

	int iGlobalSeed = 1234;
	srand((unsigned)time(0)+iGlobalSeed);

    const char* modelName = 0;
    const char* detectorName = "SURF";
    const char* descriptorExtractorName = "SURF";

    vector<Point3f> modelBox;
    vector<string>  imageList;
    vector<Vec9d>   poseListRot;
    vector<Vec3d>   poseListTr;

    if(argc < 2)
    {
        return -1;
    }

    for( int i = 1; i < argc; i++ )
    {
        if( strcmp(argv[i], "-m") == 0 )
            modelName = argv[++i];
        else if( strcmp(argv[i], "-d") == 0 )
            detectorName = argv[++i];
        else if( strcmp(argv[i], "-de") == 0 )
            descriptorExtractorName = argv[++i];
        else
        {
            printf("Incorrect option\n");
            return -1;
        }
    }

    if( !modelName )
    {
        printf("Some of the required parameters are missing\n");
        return -1;
    }

	modelName = "TempleRingModel";

	string modelFilename = format("%s\\%sCameraParameters.xml", modelName, modelName);

    Mat_<double> cameraMatrix, distCoeffs;
    Size calibratedImageSize;
    readCameraMatrix(modelFilename, cameraMatrix, distCoeffs, calibratedImageSize);

    Ptr<FeatureDetector> detector = FeatureDetector::create(detectorName);
    Ptr<DescriptorExtractor> descriptorExtractor = DescriptorExtractor::create(descriptorExtractorName);

	if(!readModelViews( modelFilename, modelName, modelBox, imageList, poseListRot, poseListTr))
    {
        printf("Can not read the model. Check the parameters and the working directory\n");
        return -1;
    }
    
    model.name = modelName;

    long startTicks = clock();
	
	build3dmodel( detector, descriptorExtractor, modelBox,
		imageList, poseListRot, poseListTr, cameraMatrix, model );

	long totalTicks = clock() - startTicks;
	cout<<endl<<"Total Time Taken: "<<totalTicks<<" ms"<<endl;

	int seconds = (totalTicks / 1000) % 60;
	int minutes = (totalTicks / (1000 * 60)) % 60;
	int hours = (totalTicks) / (1000 * 60 * 60) % 24;
	cout<<endl<<"Total Time Taken: "<<hours<<":"<<minutes<<":"<<seconds<<endl;


    /*string outputModelName = format("%s_model.xml", modelName);

	printf("\nDone! Now saving the model ...\n");
    writeModel(outputModelName, modelName, model);*/

	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB);
	glutInitWindowSize(600, 600);
	glutInitWindowPosition(100, 100);
	glutCreateWindow(argv[0]);
	init();
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(keyPressed);
	glutMainLoop();

    return 0;
}

